/*
    Copyright 2021 codenocold codenocold@qq.com
    Address : https://github.com/codenocold/dgm
    This file is part of the dgm firmware.
    The dgm firmware is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    The dgm firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "encoder.h"
#include "motor_foc.h"

#include "arm_math.h"
#include "spi.h"

tMT6825 MT6825;
tEncoder Encoder;

void ENCODER_init(void)
{
    // Init
    Encoder.raw = 0;
    Encoder.count_in_cpr = 0;
    Encoder.count_in_cpr_prev = 0;
    Encoder.shadow_count = 0;
    Encoder.pos_cpr_counts = 0;
    Encoder.vel_estimate_counts = 0;
    Encoder.pos = 0;
    Encoder.vel = 0;
    Encoder.phase = 0;
    Encoder.phase_vel = 0;

    Encoder.interpolation = 0;

    int encoder_pll_bw = 2000;
    float bandwidth = min(encoder_pll_bw, 0.25f * PWM_FREQUENCY);
    Encoder.pll_kp = 2.0f * bandwidth;           // basic conversion to discrete time
    Encoder.pll_ki = 0.25f * square(Encoder.pll_kp); // Critically damped
    Encoder.snap_threshold = 0.5f * CURRENT_MEASURE_PERIOD * Encoder.pll_ki;
}

bool ENCODER_sample(void)
{
    uint8_t data_t[2];
    uint8_t data_r[2];

    data_t[0] = 0x83;
    data_t[1] = 0x84;

    Enco_CS_Down();
    HAL_SPI_Transmit(&hspi1, &data_t[0], 1, 10);
    HAL_SPI_Receive(&hspi1, &data_r[0], 1, 10);
    Enco_CS_Down();
    Enco_CS_Up();
    HAL_SPI_Transmit(&hspi1, &data_t[1], 1, 10);
    HAL_SPI_Receive(&hspi1, &data_r[1], 1, 10);
    Enco_CS_Up();

    MT6825.angle = (data_r[0] << 6) | (data_r[1] >> 2);
	return true;
}

float normAngle(float _angle)
{
    // 取余运算可以用于归一化
    _angle = fmod(_angle, 2 * M_PI);
    return _angle >= 0 ? _angle : (_angle + 2 * M_PI);
}

void ENCODER_loop(void)
{
    ENCODER_sample();
    Encoder.raw = (ENCODER_CPR - MT6825.angle);

    int count = Encoder.raw;

    /*  Wrap in ENCODER_CPR */
    while (count > ENCODER_CPR)
        count -= ENCODER_CPR;
    while (count < 0)
        count += ENCODER_CPR;
    Encoder.count_in_cpr = count;

    /* Delta count */
    int delta_count = Encoder.count_in_cpr - Encoder.count_in_cpr_prev;
    Encoder.count_in_cpr_prev = Encoder.count_in_cpr;
    while (delta_count > +ENCODER_CPR_DIV)
        delta_count -= ENCODER_CPR;
    while (delta_count < -ENCODER_CPR_DIV)
        delta_count += ENCODER_CPR;

    /* Add measured delta to encoder count */
    Encoder.shadow_count += delta_count;

    /* Run vel PLL */
    Encoder.pos_cpr_counts += CURRENT_MEASURE_PERIOD * Encoder.vel_estimate_counts;
    float delta_pos_cpr_counts = (float)(Encoder.count_in_cpr - (int)Encoder.pos_cpr_counts);
    while (delta_pos_cpr_counts > +ENCODER_CPR_DIV)
        delta_pos_cpr_counts -= ENCODER_CPR_F;
    while (delta_pos_cpr_counts < -ENCODER_CPR_DIV)
        delta_pos_cpr_counts += ENCODER_CPR_F;
    Encoder.pos_cpr_counts += CURRENT_MEASURE_PERIOD * Encoder.pll_kp * delta_pos_cpr_counts;
    while (Encoder.pos_cpr_counts > ENCODER_CPR)
        Encoder.pos_cpr_counts -= ENCODER_CPR_F;
    while (Encoder.pos_cpr_counts < 0)
        Encoder.pos_cpr_counts += ENCODER_CPR_F;
    Encoder.vel_estimate_counts += CURRENT_MEASURE_PERIOD * Encoder.pll_ki * delta_pos_cpr_counts;

    // align delta-sigma on zero to prevent jitter
    bool snap_to_zero_vel = false;
    if (abs(Encoder.vel_estimate_counts) < Encoder.snap_threshold)
    {
        Encoder.vel_estimate_counts = 0.0f;
        snap_to_zero_vel = true;
    }

    // run encoder count interpolation
    // if we are stopped, make sure we don't randomly drift
    if (snap_to_zero_vel)
    {
        Encoder.interpolation = 0.5f;
        // reset interpolation if encoder edge comes
    }
    else if (delta_count > 0)
    {
        Encoder.interpolation = 0.0f;
    }
    else if (delta_count < 0)
    {
        Encoder.interpolation = 1.0f;
    }
    else
    {
        // Interpolate (predict) between encoder counts using vel_estimate,
        Encoder.interpolation += CURRENT_MEASURE_PERIOD * Encoder.vel_estimate_counts;
        // don't allow interpolation indicated position outside of [enc, enc+1)
        if (Encoder.interpolation > 1.0f)
            Encoder.interpolation = 1.0f;
        if (Encoder.interpolation < 0.0f)
            Encoder.interpolation = 0.0f;
    }
    float interpolated_enc = Encoder.count_in_cpr  + Encoder.interpolation;
    while (interpolated_enc > ENCODER_CPR)
        interpolated_enc -= ENCODER_CPR;
    while (interpolated_enc < 0)
        interpolated_enc += ENCODER_CPR;

    float shadow_count_f = Encoder.shadow_count;
    float turns = shadow_count_f / ENCODER_CPR_F;
    float residual = shadow_count_f - turns * ENCODER_CPR_F;

    /* Outputs from Encoder for Controller */
    Encoder.pos = turns + residual / ENCODER_CPR_F;
    UTILS_LP_MOVING_AVG_APPROX(Encoder.vel, (Encoder.vel_estimate_counts / ENCODER_CPR_F), 5);
    Encoder.phase = normAngle((interpolated_enc * M_2PI * 11) / ENCODER_CPR_F);
    Encoder.phase_vel = Encoder.vel * M_2PI * 11;
}